#ifndef PCL_FITTING_HPP
#define PCL_FITTING_HPP

// #include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>  //使用MatrixXd需引入

namespace pclTool{

enum class solveMethod{SVD,QR};

/**
 * 从点云拟合平面
 * @tparam S 点坐标的数据类型
 * @param points 输入的点云 std::vetor<Eigen::Maxtrix<S,3,1>>类型
 * @param plane_coeffs 平面参数(4维)包括： 单位法向量参数 a b c 和截距 d
 * @param eps 拟合平面的容许误差 点到拟合平面的距离平方大于该值，则拟合失败
 * @return bool 返回拟合成功与否
 */
template <typename S>
bool FitPlane(const std::vector<Eigen::Matrix<S,3,1>>& points,Eigen::Matrix<S,4,1>& plane_coeffs,double eps=1e-2,solveMethod method=solveMethod::SVD)
{
    //至少3个点才拟合成一个平面
    if(points.size()<3)
    {
        return false;
    }

    if(method==solveMethod::SVD)
    {
        Eigen::MatrixXd A(points.size(), 4);
        for (size_t i = 0; i < points.size(); ++i)
        {
            A.row(i).head<3>() = points[i].transpose();
            A.row(i)[3] = 1.0;
        }

        Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
        plane_coeffs = svd.matrixV().col(3);
    }
    else//QR
    {
        Eigen::MatrixXd A(points.size(),3);
        for(size_t i=0;i<points.size();++i)
        {
            A.row(i)=points[i].transpose();
        }
        Eigen::MatrixXd b(points.size(),1);
        b.setOnes();
        b*=-1.0;
        Eigen::Vector3d normvec=A.colPivHouseholderQr().solve(b);
        plane_coeffs.template head<3>() = normvec;
        plane_coeffs[3]=1.0;

        //归一化
        double n = plane_coeffs.norm();//注意：是对平面参数（a,b,c,d)归一化，而不是单独对法向量归一化
        if (n < std::numeric_limits<double>::min())
        {
            // 处理零模长情况（返回失败，避免除零）
            return false;
        }
        plane_coeffs.template head<3>()=normvec/n;
        plane_coeffs[3]=1.0/n;
    }
    

    //检查误差
    for(size_t i=0;i<points.size();++i)
    {
        double err= plane_coeffs.template head<3>().dot(points[i])+plane_coeffs[3];
        if(err*err>eps)
        {
            return false;
        }
    }
    return true;
}

}//namespace pclTool

#endif
